#ifndef Map_H_
#define Map_H_

//g2o
#include "g2o/core/graph_optimizer_sparse.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/math_groups/se3quat.h"
#include "./graph/vertex_se3_quat.h"
#include "./graph/edge_se3_quat.h"

//other
#include <stdio.h>
#include <string.h>
//#include "RGBDFrame.h"
#include "FrameMatcher.h"
#include "RGBDCamera.h"
#include "myutils.h"
#include <string>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/passthrough.h>
#include "Transformation.h"
#include "FrameNode.h"



using namespace std;
typedef std::vector<Transformation, Eigen::aligned_allocator<Eigen::Vector4f> > TransformationVector;
typedef std::vector<FrameNode, Eigen::aligned_allocator<Eigen::Vector4f> > FrameNodeVector;

class Map
{
	public:
	int nr_frames;
	FrameNode * first;
	FrameNode * last;
	
	FrameMatcher * matcher;
	double ** frame_similarity;
	RGBDCamera * camera;
	
	pcl::visualization::CloudViewer * view;
	
    Map();
    ~Map();
    bool addFrame(RGBDFrame * frame);
    void slam(RGBDCamera * camera);
    void slam_once(RGBDCamera * camera);
    
    Transformation * getSolidTransformation(FrameNode * mid,int offset);
    
	void addTransformation(Transformation * transformation);
	void addNode(FrameNode * node);
	g2o::EdgeSE3 * getG2OEdge(Transformation * transformation);
	g2o::VertexSE3 * getG2OVertex(FrameNode * node);
	g2o::SparseOptimizer * startNewGraph();
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr vizualizeTransformation(Transformation * transformation);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getMapCloud();

	void store(string path);
	void load(string path);
	void setViewer(pcl::visualization::CloudViewer * viewer);
	
	void clearGraph();
	void resetGraph();
	void optimizeGraph();
	void optimizeTransformation(RGBDCamera * camera);
    
    private:
	g2o::SparseOptimizer	graphoptimizer;    
};

#endif
